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OBJECTIVE 


Apply  modern  estimation  theory  to  ground-based  sensor  tracking  data  for 
evaluating  a  missile's  flight  performance.  The  emphasis  is  on  reconstruction 
of  the  missile's  free- flight  trajectory.  The  approaches  investigated  include 
the  linear  Kalman  filter  working  in  Cartesian  coordinates,  and  the  extended 
(nonlinear)  Kalman  filter  implemented  in  Cartesian  coordinates.  A  prime 
objective  was  an  analysis  of  1)  filter  initialization,  2)  Q  matrix  weighting, 
and  3)  variation  in  the  statistics  of  the  observation  tracking  sensor  data. 


RESULTS 


The  nonlinear  filter  approach  (linear  dynamics  with  nonlinear 
measurements)  provided  superior  performance  evaluation  over  a  strictly  linear 
filter  approach.  The  use  of  an  extended  Kalman  filter  greatly  enhances  the 
level  of  confidence  in  defining  (and  reconstructing)  a  missile's  free-flight 
trajectory. 


RECOMMENDATIONS 


It  is  recommended  that  a  system  engineering  effort  be  conducted  primarily 
in  the  areas  of  1)  adaptive  Kalman  filtering,  including  maneuver  gates  and 
weighted  gain  matrix;  2)  initialization,  including  timing  and  initial  values 
of  covariance  matrix,  state  vector,  and  variations  in  statistics  of  the  noise 
on  the  sensor  measurements;  and  (3)  terminal  intercept  performance,  including 
variance  of  line-of-sight  rates  measured  by  guidance  sensors. 

Secondary  areas  of  investigation  recommended  are  1)  nonlinear  dynamics; 

2)  smoothing  of  optimal  estimates;  and  3)  multiple  sensor  observations. 
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1.  ESTIMATION  THEORY  AS  A  TOOL 


Modern  estimation  theory  is  one  of  the  primary  tools  used  to  evaluate 
missile  performance.  Modern  day  missiles  are  tested  in  free-flight 
environments  at  well -instrumented  test  ranges.  Sensors  gather  information 
about  the  missile  as  it  is  being  tested  in  free  flight.  Estimation  theory 
plays  an  important  role  in  evaluating  the  observation  information  gathered  by 
the  range  instrumentation  sensors.  The  information  is  processed  in  a 
computerized  data  processing  system.  The  data  processing  system  performs  a 
number  of  operations  such  as:  (1)  trajectory  reconstruction  of  missile  and 
target,  (2)  reconstruction  of  the  missile  attitudes  and  attitude  rates 
throughout  the  flight,  (3)  determination  of  line-of-sight  rates  as  seen  by  the 
missile  sensor  systems,  (4)  comparison  of  commanded  versus  achieved 
accelerations,  (5)  determination  of  the  guidance  and  control  steering  commands 
which  should  compare  to  those  generated  by  the  missile  flight  control  section, 
(6)  comparison  of  measured  line-of-sight  rates  as  seen  by  the  missile  sensor 
system  to  the  actual  line-of-sight  rates,  (7)  determination  of  the  ability  of 
the  sensor  to  sort  signals  from  noise  and  clutter,  and  (8)  basic  establishment 
of  missile  subsystem  performance  levels.  This  data  analysis  is  utilized  to 
establish  the  total  missile  system  performance  and  to  compare  the  free-flight 
performance  against  performance  specifications. 

ANALYSIS  APPROACH 

Reconstruction  of  the  missile  and  target  trajectories  using  modern  day 
estimation  theory,  as  emphasized  in  this  report,  is  the  key  element  in 
analyzing  the  missile's  flight  performance.  Uncertainties  in  the  measurements 
made  by  range  sensors  and  system  dynamics  can  be  modeled  and  accounted  for  by 
using  the  mathematics  of  probability  and  statistics  (ref  1).  Figure  1  is  a 
flow  diagram  of  the  flight  data  analysis  process. 

This  report  describes  an  analysis  dealing  primarily  with  optimal 
estimation  theories,  and  mathematical  tools  for  multivariate  analysis  (ref  2, 
3,  and  4)  applied  to  reconstruction  of  missile  free-flight  trajectory.  The 
approach  is  to  generate  a  trajectory  of  a  missile  in  free-flight  using  a 
six-degree  of  freedom  simulation.  Sections  3  and  6,  and  Appendix  A  present 
the  details  of  this  approach.  The  trajectory  of  the  simulated  missile  flight 


1.  Nahi,  Nasser  E. ,  Estimation  Theory  and  Applications,  John  Wiley  and  Sons, 
Inc. ,  New  York,  N.Y. ,  1961. 

2.  Green,  Paul  E. ,  Mathematical  Tools  for  Applied  Multivariate  Analysis, 
Academic  Press,  Inc.,  1978. 

3.  Chen,  Chi-Tsong,  Introduction  to  Linear  Systems  Theory;  Holt,  Rinehardt, 
and  Winston,  Inc. ,  1970. 

4.  Jazwinski,  Andrew  H. ,  Stochastic  Processes  and  Filtering  Theory,  Academic 
Press,  New  York,  N.Y. ,  1970. 
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ght  data  analysis  process. 


was  observed  using  simulated  ground-based  sensors.  The  sensor  data  was 
processed  through  an  optimal  estimation  sequential  tracking  filter. 


Sections  2  and  3  define  the  reference  frames,  the  models  used  in  the 
study,  and  the  formulation  of  the  problem.  Section  4  provides  an  overview  of 
optimal  estimation  theory.  Section  5  defines  alternative  sequential  filter 
approaches  for  determining  optimum  estimates  of  the  missile  system  state 
vector  throughout  the  flight  trajectory.  A  recommended  approach  is  presented 
which  is  essentially  an  extended  Kalman  filter  working  in  Cartesian 
coordinates. 

Sections  5  and  6  are  detailed  descriptions  of  the  filter  that  was  modeled 
in  a  computer  program  and  presents  the  analysis  that  was  performed  using  this 
model.  The  analysis  covered  the  following  aspects  of  the  problem:  (1) 
initialization,  (2)  convergence,  (3)  mismodeled  noise,  (4)  mismodeled  system 
dynamics,  (5)  transformation  noise,  (6)  data  rates,  and  (7)  advanced  concepts 
relative  to  filter  stability  and  rate  of  convergence. 

Sections  6  and  7  present  results,  conclusions  and  recommendations. 
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2.  SYSTEM  REFERENCE  FRAMES 


This  section  defines  the  reference  frames  of  the  ground-based  tracking 
sensors  (defined  for  the  analysis  as  the  inertial  frame)  and  the  reference 
frames  of  the  missile  and  of  the  target.  The  coordinate  transformations  that 
translate  elements  of  the  state  vector  from  one  frame  to  another  are  defined 
in  Appendix  B.  The  analysis  for  this  study  used  one  ground-based  tracking 
sensor  in  the  inertial  coordinate  system.  In  reality  there  would  typically  be 
a  number  of  ground-based  tracking  sensors.  The  observation  data  of  each 
individual  tracking  sensor  would  then  be  transformed  to  the  inertial 
coordinate  frame.  The  estimated  values  of  the  missile  state  vector  for  the 
missile  throughout  the  flight  trajectory  would  be  the  resultant  of  combining 
the  observation  data  from  all  of  the  ground-based  tracking  sensors. 

The  inertial  coordinate  system  is  fixed  to  the  surface  of  the  earth  with 
the  positive  Z  axis  directed  towards  the  earth's  center.  The  XT  axis  is 
aligned  parallel  with  the  missile's  initial  launch  direction.  xThe 
ground-based  tracking  sensor  (one  was  used  for  this  analysis)  is  placed  at  the 
origin  of  the  inertial  frame.  The  missile  reference  system  is  fixed  at  the 
missile's  center  of  gravity.  The  X„  axis  coincides  with  the  longitudinal  axis 
while  the  Yg  and  Zg  axes  are  directed  along  the  pitch  and  yaw  axes, 
respectively.  TheDinertial  and  missile  body  reference  frames  are  illustrated 
in  figure  2. 

The  observation  data  obtained  by  ground-based  sensor  systems  is  range, 
range  rate  (if  available),  azimuth,  and  elevation  angles.  These  variables  are 
represented  as  R,  R,  0  and  0,  respectively.  It  is  immediately  apparent  that 
the  observation  data  is  a  nonlinear  function  of  the  state  variables  as  defined 
in  a  Cartesian  coordinate  system.  For  the  purposes  of  this  study  the  range 
rate,  R,  was  not  utilized.  The  three  remaining  variables  R,  0  and  0  are 
illustrated  in  figure  3.  The  observation  data  is  defined  as 

z(tk)  =  h(xc(tk))  +  v(tR)  (1) 

where  z(tk)  is  the  P  x  1  observation  vector.  Vectors  in  this  report  will  be 
denoted  as  a  lower  case  alphabetical  letter  underlined  with  a  bar  "x. " 

In  (1),  h(x  ( t. ) )  is  the  transformation  function  that  relates  R,  0  and  0, 
which  are  the  positron  elements  of  the  state  vector  in  spherical  coordinates 
(x  ),  to  the  three  position  elements  x  ,  y  and  z  of  the  state  vector  in 
Cartesian  coordinates.  These  functional  relationships  are  defined  in  (2) 
through  (7) 


xc  = 

R  COS0COS0 

(2) 

yc  = 

R  sin0cos0 

(3) 

zc  = 

-R  sin0 

(4) 

R  = 

(xc2  +  yc2  +  zc2)h 

(5) 

0  = 

arctan  (yc/xc) 

(6) 

0  = 

-arctan[z  (x  2  +  y J2)'*5] 

(7) 
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Figure  2.  Missile  body/inertial  reference  frames 


3.  MODELS  AND  PROBLEM  FORMULATION 


This  section  presents  a  brief  description  of  the  methodology,  problem 
formulation,  and  the  models  that  were  used  to  generate  the  data  for  this 
study.  Figure  4,  an  overview  of  the  methodology,  presents  an  expanded  view  of 
the  anaysis  and  studies  that  would  comprise  a  comprehensive  missile  trajectory 
estimation  analysis  effect.  Only  the  shaded  areas  on  the  figure  were  covered 
in  the  current  phase  of  the  optimal  estimation  of  missile  free-flight 
performance  study.  The  maneuver  gate,  Kalman  gain  weighting,  guidance  command 
comparison  and  estimation  of  line-of-sight  rate  during  termir  flight 

trajectory  phase  were  not  addressed  in  this  study.  These  an.  of  study  will 

be  addressed  in  a  follow-on  study  (as  identified  in  figure  4). 

The  approach  taken  was  to  estimate  a  six-element  state  vector.  The 
six-element  state  vector  is  composed  of  position  and  velocity  elements  and  is 
written  as  follows: 


The  inertial  elements  are  in  a  Cartesian  coordinate  system. 


Missile  accelerations  are  assumed  to  be  measurable  quantities  and  would 
be  available  as  deterministic  forcing  for  the  Kalman  filter.  These 
accelerations  would  be  measured  by  missile  on-board  accelerometers. 

Similarly,  the  target's  trajectory  would  be  estimated  by  a  six-element  state 
vector  where  again  it  would  be  assumed  that  on-board  target  drone 
accelerometer  information  would  be  available  for  deterministic  forcing. 

Let's  clarify  what  has  been  stated.  This  can  be  seen  in  terms  of 
mathematical  equations,  and  for  brevity  only  the  missile  will  be  discussed.  A 
parallel  set  of  equations  would  describe  the  optimal  estimation  of  the  target 
drone  trajectory.  The  dynamics  of  the  system  (missile  system)  in  free  flight 
can  be  defined  in  terms  of  the  following  nonlinear  stochastic  differential 
equation  (ref  4  and  5): 

x(t)  =  f(x,u,t)  +  w(t), 


x(tQ)~  N(x(to),  P(tQ))  (9) 

where  x  is  the  n-vector  of  state  variables  describing  the  system,  u  is  an 
n-vector  of  time-dependent  forcing  functions,  w(t)  is  an  n-vector  of  white 
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process  noise,  t  is  the  independent  variable,  time,  and  f  is  a  known  function 
of  x,  u  and  t.  The  statistics  on  w(t)  are 

E{w(t)}  =  0  (10) 


and 


E{w(t)  wT(t)}  =  Q(t)6(t-r)  (11) 

where  Q(t)>0  is  the  process  noise  spectral  density  matrix. 

5 

The  discrete  nonlinear  observations  obtained  by  the  ground-based  sensor 
systems,  which  are  taken  at  time  instants  tk,  can  be  expressed  as 

z(tk)  =  h(x(tk))  +  v(tk)  (12) 

where  z  is  the  p-vector  of  observations  of  the  state  x,  h  is  the  p-vector  of 
nonlinear  functions  which  relate  the  state  and  observations,  and  v  is  a 
p-vector  of  white  measurement  noise  with  statistics 

E{v(tk)}  =  0  (13) 


and 


E{y(tk)yT(tk)}  =  R(tk)  (14) 

The  quantity  R( tk)  is  the  measurement  noise  covariance  matrix  which  is 
restricted  to  be  positive  semidef inite. 

Equation  (9)  describes  the  generalized  system,  including  the  nonlinear 
effects  in  the  dynamics  which  relate  to  "real  world"  situations.  One  of  the 
assumptions  in  this  study  is  that  the  data  rates  for  the  accelerations  are 
high  enough  that  a  linearized  formulation  of  the  system  dynamics  can  be 
realized.  In  essence,  the  acceleration  is  assumed  to  be  constant  over  an 
update  interval  (interval  between  data  updates  of  the  on-board 
accelerometers).  It  is  also  assumed  that  the  system  dynamics  are  time 
invariant.  Based  on  these  assumptions  equation  (9)  is  rewritten  as 

x(t)  =  Fx(t)  +  Gu(t)  +  w(t)  (15) 

where  the  solution  of  (16)  is  given  as  (reference  3  page  131) 


5.  Applied  Optimal  Estimation,  written  by  Technical  Staff  of  the  Analytic 
Sciences  Corporation,  edited  by  Arthur  Gelb,  The  MIT  Press,  1974. 


9 


(16) 


t 


x(t)  =  <Kt,  t0)x(tQ)  +  / 

to 


4>(t,t)  Gu(i)dt 


t 

+  L  <*>(t,T)  w( t )dt 
*0 

where  <J>(t,r)  is  the  state  transition  matrix  of  x(t)  =  Fx(t);  or, 
correspondingly,  the  unique  solution  of 

«Kt,T)  =  F4>(t,i),  (17) 


♦(t,t)  =  I 


The  F  matrix  is  defined  as 


'0 

0 

0 

1 

0 

0‘ 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

(18) 


which  is  the  system  matrix  and  the  G  matrix  is  defined  as 


0  0  0 
0  0  0 
10  0 
0  10 
.0  0  1 


(19) 


which  is  the  distribution  matrix  for  the  forcing  function,  missile 
acceleration,  defined  as 

u(t)  =  [ax  ay  az]T  (20) 

For  the  system  model,  the  forcing  function  is  the  missile  acceleration  as 
measured  by  accelerometer  sensors  in  the  missile  body  axis  system.  The 
sensors  measure  with  an  error  which  is  defined  as  the  vector  w(t),  where 

w(t)  =[000  6ax(t)  6  (t)  6az(t)]T  (21) 

The  state  transition  matrix  whichis  the  solution  of  (17)  can  be  solved  using 
the  inverse  LaPlacian  operator  11 L  A" 

<Kt,t0)  =  L_1[(SI  -  F)'1]  (22) 

For  the  system  defined  by  (15)  where  F  is  given  in  (18),  and  when  the  system 
is  discretely  observed,  the  state  transition  matrix  has  the  closed  form 
solution 
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4>(A)  = 


(23) 


1  0  0  A  0  0 
0  1  0  0  A  0 
0  0  1  0  0  A 
0  0  0  1  0  0 
0  0  0  0  1  0 
0  0  0  0  0  1 

where  A  Is  the  sampling  interval  for  the  discretely  observed  continuous  system 
of  (15). 

We  now  have  enough  information  to  form  an  expression  for  the  sampled 
version  of  (16): 


x(K  +  1)  =  <HA)x(K)  +  /  k+1  4>(t)Gu(t)dt  (24) 


Equation  (24)  is  termed  as  the  update  equation  and  where  dealing  with  a 
stochastic  system,  the  update  is  in  terms  of  the  optimum  estimate  of  the  state 
vector  x  at  the  Kth  interval.  Since  the  forcing  function  is  assumed  to  be 
constant  over  the  observation  interval  (assuming  a  high  data  rate/sampling 
rate  on  the  missile  acceleration)  equation  (24)  can  be  written  as 


x(K+l)  =  <t>(A)  x(K)  +  T(A)  u(K) 


where 


T(A)  = 


Y  0  0 
0  0 
o  o  y~ 

A  0  0 
0  A  0 
L  0  0  A  J 


(25) 


(26) 


At  this  stage  the  question  arises,  "why  have  we  expended  the  efforts  to  define 
system  dynamics  as  expressed  in  the  previous  development—equations  (9) 
through  (26)?"  This  question  has  two  answers:  First,  the  filter  noted  in 
figure  (4)  that  will  be  utilized  to  formulate  optimum  estimates  of  the  state 
vector  will  be  a  sequential  Kalman  filter.  The  estimates  of  x  will  be  termed 
as  x  where  the  """  above  the  vector  denotes  an  estimated  valui  of  the  vector. 

A  generalized  model  (ref  5,  page  111)  of  a  system  model  and  a  discrete  Kalman 
filter  is  illustrated  in  figure  5. 
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Figure  5.  System  model  and  discrete  Kalman  filter. 


The  Kalman  filter  illustrated  in  figure  5  requires  a  description  of  the 
system  dynamics.  Figure  5  does  not  represent  the  Kalman  filter  utilized  in 
the  tracking  problem  of  figure  4,  it  merely  represents  a  generalized  Kalman 
filter  to  illustrate  the  point  that  the  Kalman  filter  needs  to  have  a 
knowledge  of  the  system  dynamics. 

The  second  answer  to  the  question  relates  to  the  fact  that  the  analysis 
was  performed  on  a  digital  computer  and  required  a  sampled  data  representation 
of  the  system.  Having  defined  the  system  dynamics  and  a  concept  definition 
and  approach  (as  illustrated  in  figure  4)  the  next  step  was  to  define  the 
sequential  filter  that  will  derive  optimum  estimates  of  the  state  vector. 

This  is  a  multi-faceted  problem;  therefore,  we  approach  this  through  a 
sequence  of  logical  tradeoffs  and  analysis.  These  steps  are  as  follows: 

1.  Review  optimal  estimation  theory.  (This  is  an  overview  with 
reference  made  to  the  open  literature  for  the  details.) 

2.  Investigate  alternate  approaches;  and 

3.  Analysis  of  recommended  approaches. 

These  steps  are  detailed  in  the  following  three  sections. 
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4.  OPTIMAL  ESTIMATION  THEORY 


This  section  presents  a  few  fundamental  ideas  on  optimal  estimation 
theory.  The  emphasis  is  focused  on  nonlinear  minimum  variance 
estimation/extended  Kalman  filter  theory. 

An  estimate,  x  is  the  computed  value  of  a  quantity,  x,  based  on  a  set  of 
measurements,  z.  An  unbiased  estimate  is  one  where  expected  value  is  the  same 
as  that  of  the'quantity  being  estimated.  A  minimum  variance/unbiased  etimate 
has  the  property  that  its  error  variance  is  less  than  or  equal  to  that  of  any 
other  unbiased  estimate.  A  consistent  estimate  is  one  which  converges  to  the 
true  value  of  x,  as  the  number  of  measurements  increases.  We  shall  then  look 
at  unbiased,  minimum  variance,  consistent  estimators. 

The  open  literature  (references  5,  6,  7,  8,  10)  present  details  on 
various  types  of  estimators.  As  an  example  ref  5  develops  the  theory  for  the 
least-squares  and  weighted-least-squares  estimators.  These  are  summarized  as 
follows.  The  measurement  process  is  modeled  as 


z  =  Hx  +  v 

(27) 

where  z  is  an  £  x  1  vector,  x  is  on  n  x  1  vector  and  H  is  an 
v  is  an  £  x  1  vector.  The  least  squares  estimator  minimizes 

£  x  n  matrix 
the  quantity 

J  =  (z  -  Hx)T  (z  -  Hx) 

(28) 

The  estimate  is  found  by  setting 

9J  .  n 

(29) 

which  results  in  the  estimate 

x  =  (HTH)-1HTz 

(30) 

6.  Frank  J.  Seiler  Research  Laboratory,  SRL-TR-72-0004,  An  Engineer's  Guide 
to  Building  Nonlinear  Filters,  Vol  1  and  2;  Richard  S.  Bucy,  Calvin 
Hecht,  and  Capt.  Kenneth  P.  Semme,  May  1972. 

7.  NELC  TR  1967,  Covariance  Analysis  of  the  DD963  Navigation  System,  by 
Jeffrey  M.  Nash,  November  1975  (NELC  is  now  NOSC). 

8.  MTR-2417,  Understanding  Kalman  Filtering  and  Its  Application  in  Real  Time 
Tracking  Systems,  by  J.  J.  Burke,  the  Mitre  Corporation,  Bedford,  Mass., 
July  1972. 

10.  Mendel,  Jerry  M. ,  Discrete  Techniques  of  Parameter  Estimation,  Marcel 
Dekker,  Inc.,  New  York,  N.Y.,  1973. 
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(31) 


The  weighted-least-squares  estimate  is  formed  by  minimizing 

J  =  (z  -  Hx)TR-1(z  -  Hx) 

which  results  in  the  estimate 

x  =  (HTr'1H)"1  HTR_1z  (32) 

The  estimators  described  by  equations  (30)  and  (32)  are  referred  to  as  Batch 
Processors.  These  estimators  require  that  the  observation  data  be  stored. 
However,  they  are  not  sequential  filters. 

4.1  KALMAN  FILTER  EQUATIONS  (LINEAR) 

A  recursive  filter  is  one  in  which  there  is  no  need  to  store  past 
measurements  for  the  purpose  of  computing  present  estimates.  This  is 
basically  the  idea  behind  the  Kalman  filter.  A  timing  diagram  for  a  discrete 
Kalman  filter  is  presented  in  figure  6.  This  diagram  illustrates  the 
propagation  or  extrapolation  of  both  the  state  estimate  x. (-)  and  the  error 
covariance  P.  (-).  The  extrapolated  values  of  the  state  estimate  and  error 
covariance  are  updated  across  each  new  measurement  at  discrete  increments  in 
time. 


HK-1  •  Rk-1.  ZK-1  Hk.  RK.  Zk  (SEE  NOTE  BELOW) 


Note:  the  following  convention  for  the  notation  will  be  assumed: 
-k-l(~)  ’s  an  ec!uiva1ent  form  of 
-k-l(t)  ’s  an  eQuiva16nt  form  of 

Pk-l(~)  1s  an  eclu1valent  f°rm  °f  P(tk_i~) 
etc. 

Figure  6.  Discrete  Kalman  filter  timing  diagram. 


A  summary  of  the  discrete  Kalman  filter  equation  is  outlined  in  table  1.  The 
initial  conditions  and  other  assumptions  are 


E[x(0)]  =  x(0) ,  E[(x(0)  -  x(0)(x(0)  -  x(0))T]  =  PQ 
E[wkVjT]  =  0  for  all  j,  k 


(33) 


System  Dynamics 

X(t)  «  F  X(t)  +  GU(t)  +  W(t),  W(t)  ~  N(O.Q(t)) 

State  Transition 
Matrix 

♦(‘K^K-l)  *  F*CtK.t|C.l);*(tK-l*tK-l)“  1 

Measurement  Model 

?KsHKxK  +  vKl  vk~n(0,rk) 

State  Estimate 
Extrapolation 

X(tK  - )  =  *(tK,tK.  i )  X(t|c- 1  +) 

+  .Ak-1  <^(tK*r)  G(t)  U(r)  dr 

Error  Covariance 
Extrapolation 

P(tK-)  =  *  (tK,tlc.,)  P(tK., +)  *T(tK,tK.1) 
rt  k 

+  K-l  *(tK»T)Q(T)+T(t|C.T)dT 

State  Estimate 

Update 

X(tK+)  =  X(tK-)  +  K(tK)  [Z(tK)  -  H(tK)X(tK-)J 

Error  Covariance 
Update 

P(tK+)  =  U-K(tK)H(tK))P(tK-) 

Kalman  Gain 

Matrix 

XOr)  =  POK-)HT(tK)[H(tK)P(tK-)HT(tK)  +  R(tK)pl 

Table  1.  Kalman  filter  equations  (linear  dynamics  and  linear 
measurements) . 


4.2  NONLINEAR  MINIMUM  VARIANCE  ESTIMATION  (EXTENDEO  KALMAN  FILTER) 

The  equations  as  outlined  in  table  1  are  for  a  system  described  by  linear 
dynamics  and  where  the  measurements  are  linear  also.  In  reality,  for  most  of 
the  practical  situations,  neither  the  dynamics  nor  the  measurements  are 
linear.  The  Kalman  filter  is  an  algorithm  where  the  conditional  mean  can  be 
computed  from  a  unique  1 i near  operation  on  the  measurement  data.  For  the  more 
general  case  described  by  the  nonlinear  stochastic  differential  equation 

i(t)  =  f(x(t),t)  +  w(t)  (34) 
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and  x(t)  is  estimated  from  sampled  nonlinear  measurements  of  the  form 

zk  =  hk<*(tk»  +  -k  (35) 

the  problem  of  smoothing  and  filtering  becomes  considerably  more  difficult. 

For  the  linear  gaussian  case,  the  optimal  estimate  of  x(t)  for  most  reasonable 
Bayesian  optimization  criteria  is  the  conditional  mean.  By  contrast,  in  the 
nonlinear  problem  x(t)  is  generally  not  gaussian;  hence  many  Bayesian 
criterion  (ref  6)  lead  to  estimates  that  are  different  from  the  conditional 
mean.  More  often,  the  nonlinear  systems  cannot  be  expressed  in  closed  form, 
therefore  methods  of  approximating  optimal  nonlinear  filters  must  be  devised. 
The  remainder  of  this  section  will  elaborate  on  the  extended  Kalman  filter 
(one  methodology  for  approximating  optimal  nonlinear  filters).  The  covariance 
matrix  is  defined  as 

P(t)  £  E  ^x(t)  -  x(t)][x(t)  -  x(t)]J  T  (36) 

where  the  differential  equation  of  P(t)  is  given  as 


=  E  [x 


P(t)  =  E  [x(t)  -  x(t)][x(t)  -  x(t)]' 


+  [X(t)  -  x(t)][x(t) 


-  X(t)]TJ 


(37) 


x(t)  =  f(x(t),t) 
x(t)  =  f(x(t),t)  +  w(t) 


(38) 

(39) 


Working  only  with  the  first  terms  of  equation  (37)  and  where  the  dependence  of 
x  on  t  and  f  and  on  x  and  t  is  suppressed  for  notational  convenience  we  have 

E[(f  -  f  -  w)(x  -  x)T] 

*  T  *  T  T  T 

=E[fx  -fx'-fx  *fx' 


-  wxT  +  wxT] 


(40) 


,~>.T  ^.T  "  T  /NT 

[fx1  -  fx'  -  fx1  +  fx1 


A  A  T  .  /NT 

-  wxT  +  wx 
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=  -  fxT  +  +  wx^ 


(41) 


remembering  that 


x  =  x(tk_1)  +  S  f(x(t),t)dt 
tk-l 


(42) 


+  f  w(x)dT 
t, 


'k-1 


E[wxT]  =  E[w(xT(tk_1)  +  f  fT(x(i),x)dt 

tk-l 


+  I  w  (t )dt ) ] 
t, 


(43) 


'k-1 

where  E[w]  =  0 

Thus  the  first  two  terms  of  (43)  drop  out  and  the  only  term  left  is 
t 


E(w(t)  /  wT(r)dr] 
t, 


(44) 


VI 


Interchanging  the  integration  and  expectation  operation  and  evaluating  we  get 
wxT  =  1/2Q  (45) 

where 

E[w(t)wT(T)]  =  Q(t)  6  (t-x)  (46) 

Looking  now  at  the  second  terms  of  equation  (37): 


E[[x(t)  -  x(t)]  [A(t)  -  x(t)]T] 


(47) 


and  again  making  the  substitutions  for  x  and  x  into  (47)  and  using  simplified 
notation  as  mentioned  above  we  get 


r  r  A  ^  T  A  *  T  xT  ‘  I  A 

E[xx  -  xx  -  xx  +  xx  ] 


(48) 


=  E[xfT  -  x(fT  +  wT)  -  xfT  +  x(fT  +  wT)] 
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(49) 


=  xfT  -  xfT  -  xwT  -  xfT  +  E[xfT]  +  E[xwT] 


~T  AT 

=  -  xf1  +  xf1  +  1/2Q 

Combining  the  results  of  (41)  and  (49)  we  get  the  composite  or 


•  /NT  "  T 

P  =  f x  -  fx1  -  xf 1  (50) 

+  xf  +  Q 

Equation  (50)~is  the  desired  result,  and  inspection  of  (50)  shows  that  P 
is  a  function  of  f  which  depends  on  the  probability  density  function  p(x,t)  or 


f(x,t)  =  /  f(x,t)  p(x,t)dx1-  .  .  dxn  (51) 

-00 

Thus,  to  compute  f(x,t),  the  probability  density  p(x,t)  must  be  known.  An 
alternative,  when  full  information  is  not  known  about  the  probability  function 
p(x,t),  is  to  linearize  the  system  dynamics.  One  methodology  is  to  use  a 
Taylor  series  expansion  of  f(x,t)  and  truncate  the  series  after  several  terms. 
This  will  be  referred  to  as  Quasi  Linearization. 

Define 

x(t)  =  f(x,t)  +  F(x  -  x)  +  w(t)  (52) 


x(t)  =  f(x,t)  =  f(x,t) 


P(t)  =  E  ^(f(x,t)  -  f(x,t)  -  F(x  -  x)  -  w(t)) 
(x(t)  -  x(t))T'  +  E^(x(t)  -  x(t)) 

(f(x,t)  -  f(x,t)  -  F(x  -  x)  -  w(t))Tj 

=  E^-F(x  -  x)  -  w(t))  (x(t)  -  x(t))T] 

[(set,  -  x(t))  (-F(x  -  X)  -  w(t))T] 


+  E 


P(t)  =  FP  +  1/2Q  +  PF1  +  1/2Q 


(53) 


(54) 

(55) 
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x(t)  =  f(x,t)  (56) 

Equations  (54)  and  (55)  are  approximate  expressions  for  propagating  the 
conditional  mean  of  the  state  and  its  associated  covariance  matrix.  (For  more 
details  see  ref  4.)  These  are  the  propagation  equations  of  the  filter 
algorithm.  To  complete  the  algorithm  update  equations  are  required  which 
weight  the  observation  data.  The  Kalman  filter  algorithm  formulates  an  update 


as  a  linear  function  of  the  measurement 

x(tk+)  =  a(tk)  +  K(tk)  z(tk)  (57) 

The  estimation  errors  are  defined  as 

£(tk+)  £  x(tk+)  -  x(tR)  (58) 

e(tk-)  -  x(tk-)  "  xUfc)  (59) 

Combining  (57),  (58)  and  (59)  yields 

£(tk+)  =  a(tk)  +  K(tk)  h(x(tk))  +  K(tk)  v(tfc)  (60) 

+  e(tk-)  -  x(tk-) 

Utilizing  the  fact  that  the  estimate  is  unbiased,  ie. ,  that  E[e(t.+)]  =  0  an 
that  K 

E[e(tk-)]  =  0  (61) 

and  E[v(tk)]  =  0 

an  expression  for  a(tk)  can  be  solved  for  using  (60).  This  expression 
for  a(t.),  is  formulated  by  taking  the  expectation  of  (60)  and  applying  the 


conditions  of  (61).  This  resultant  expression  for  a(tk), 

a(tk)  =  x(tk-)  -  K(tk)  h(x(tk))  (62) 

is  substituted  into  (57)  which  yields  an  expression  for  the  update  estimate 

x(tk+)  =  x(tk-)  +  K(tk)  [z(tk)  -  h  (x(tk)) J  (63) 

An  expression  for  the  estimation  error  can  also  be  formulated  using  (60)  and 
(62) 
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£(tk+)  =  e(tk")  +  K(tk)  h^x(tR)j  -h^x(tk>^  +  K(tk)  v(tfc)  (64) 

The  optimal  gain  is  required  which  will  yield  an  estimate  (the 
approximate  conditional  mean  of  x(t))  which  is  a  minimum  variance 
estimate--one  that  minimizes  the  class  of  functions 

J(tk)  =  E[e(tk+)T  S  e(tk+)]  (65) 

for  any  positive  semidefinite  matrix  S.  By  choosing  S  to  be  the  identity 
matrix  I,  then 


J(tk)  =  E[e(tk+)  e(tk+)]  =  trace  [P(tk+)] 


where 

P(tk+)  =  E[c(tk+)  e(tk+)T] 

By  expanding  (67)  out  and  taking  the  trace  and  solving  the  equation 

3J(V  =  n 

aKry 

for  K(tk)  yields 


;k)  =  -E[s(V)[h(x(tk))  -h(x<tk))]T 
x|E[[h(x(tk))  -  h(x<tk))][t>  (x(tk>) 

-  5(x<tk))]T  *  RU,)}"1 


P(tk*)  =  P(tk-)  *  K(tk)E^h|x(tk)l  -  h((x(tk))]E(tk-)TJ  (70) 

Equations  (69)  and  (70)  are  impractical  to  implement  because  of  their 
dependence~upon  the  probability  density  function  for  x(t)  which  is  required  to 
calculate  h(t. ).  A  solution  would  be  to  expand  h(x(t. ))  in  a  Taylor  series 
about  x(tk).  K 


where 


H(x(tk-))  = 

9x  X  =  x(tk-) 


Truncating  the  series  of  equation  (71)  after  two  terms  and  substituting  the 
approximation  for  h(x(t^))  into  equations  (69)  and  (70)  and  the  equation 

which  is  obtained  from  the  approximation  of  (71)  results  in  the  extended 
Kalman  filter  update  equation  as  follows: 

x(tR+)  =  x(tk-)  +  K(tk)jz(tk)  -  h^x(tk-))J  (74) 

K(V  =  P(tk_)  HTU(tk_))  [H(^(tk‘))P(tk')  hT(*(V})  +  R(tk}]  1  (75) 

P(tk+)  =  [i  -  K(tk)  H(x(tk-))]  P(tk-)  (76) 

Equations  (53),  (55),  (74),  (75),  and  (76)  constitute  the  extended  Kalman 
filter  equations.  These  equations  are  summarized  in  table  2. 


)= ^(v) 


(73) 


System  Dynamics 

X(t)  =  f(X(t).t)  +  W(t);  W(t)~  N(O.Q(t)) 

Measurement 

Model 

Z(tK)  =  h(X(tK))  +  V(tK) 

Initial  Conditions 

Assumption 

X(O)  '  N(X0,  PD) 

E  [W(t)V(tK)T]  =  o  for  all  t  and  all  k 

State  Estimate 
Propagation 

X(t)  =  f(X(t).t) 

Error  Covariance 
Propagation 

P(t)  =  F(X(t,t)  P(t)  +  P(t)  FT(X(t),t)  +  Q(t) 

State  Estimate 
Update 

Error  Covariance 
Update 

Gain  Matrix 

XK(+)=  XK(-)  +  Kk  (ZK  -  hK(XK(-))) 

pK(+)=  U-kkhK(XK(-))]  pK(-> 

*k  =  Pk(-)Hkt(Xk(-))[hk(Xk(-))Pk(-)* 

hkt(Xk(-))  +  Rk1_1 

Definitions 

F(X(t),t)  =  1  S<«>  =  *0> 

<»  hv(X(tir )) 

HX(X(-))=  fx(tK)  »*»)“«-) 

Table  2.  Extended  Kalman  filter  equations. 


5.  ALTERNATE  APPROACHES 


This  section  describes  the  alternate  approaches  to  formulating  the  Kalman 
filter  for  estimating  the  state  variables  which  define  the  trajectory  of  a 
missile  during  free  flight.  Two  types  of  filters  were  investigated  for  the 
tracking  algorithm:  (1)  strictly  linear  (linear  dynamics  and  linear 
measurements)  in  Cartesian  coordinates,  and  (2)  nonlinear  (linear  dynamics  but 
nonlinear  measurements)  implemented  in  Cartesian  coordinates.  A  nonlinear 
filter  implemented  in  spherical  coordinates  was  also  investigated. 

5.1  LINEAR  FILTER  APPROACH 

The  first  approach  (strictly  linear  filter  in  Cartesian  coordinates) 
utilized  a  noise  variance  transformation  methodology  to  allow  the  filter 
observation  to  be  linear.  The  state  vector  is  defined  as 


x(t)  = 


x 

X 


y 

y 


z 


z 


and  the  state  transition  matrix  defined  as 


(77) 


4>  = 


"  1  A  0  0  0  0  ' 
0  1  0  0  0  0 
0  0  1  A  0  0 
0  0  0  1  0  0 
0  0  0  0  1  A 
.  0  0  0  0  0  1  . 


(78) 


Burke  (ref  8)  outlines  a  methodology  for  decoupling  the  filter.  That  is,  have 
three  parallel  filters  with  state  transition  matricies  of  order  2x2  rather 
than  one  filter  with  a  6  x  6  state  transition  matrix.  There  is  some  loss  of 
information  by  going  to  the  decoupled  f i lters--some  of  the  cross  terms  in  the 
covariance  matrix  are  lost.  Since  all  three  filters  of  the  decoupled 
arrangement  are  essentially  identical,  only  one  will  be  elaborated  upon.  The 
x  component  will  be  used  as  the  example  to  define  the  algorithm.  Analogous  y 
and  z  filter  are  similarly  derived.  The  state  vector  (for  x  element)  is  now  a 
2x1  vector. 


x(t)  = 


(79) 
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where  the  state  transition  matrix  is 

-  [“] 

A  is  the  sampling  time  for  the  discrete  system  dynamics. 

x(tk)  =  <Ktk,  k-l)  x(tk-l)  +  /  K  ♦(tk,x)Gu(x)dx 

tk-l 

where 

:] 

and 

u(t)  =  ax(t)  +  6a  (83) 

ax(t)  is  the  missile  lateral  acceleration  in  the  ground  inertial  frame  and  6 
is  the  noise  component.  a 

The  translation  accelerations  in  the  missile  body  system  are 

q  =  (u  ♦  Qw  +  Rv  -  Gx)  (84) 

C2  =  (0  ♦  Ru  -  Pw  -  Gy) 

C3  =  ("  +  Pv  '  Qu  "  Gz) 

The  quantities  u,  v,  w,  and  P,  Q  and  R  are  the  rectilinear  and  angular 
accelerations  of  the  missile  (missile  body  coordinates)  as  defined  in  ref  (9). 
Gx,  G  ,  and  G  are  components  of  gravity  in  x,  y,  and  z  directions, 
respectively. 

The  translational  acceleration  components  in  the  inertial  frame  are 
ax  =  *1  A11  +  ^2  a21  +  ^3  a31 

ay  =  ^1  A12  +  ^2  a22  +  ^3  a32  (85) 

az  =  &1  A13  +  ^2  a23  +  ^3  a33 

the  A.,  terms  are  defined  in  Appendix  A.  The  6  term  in  (83)  is  the 
nondetermini stic  portion  of  the  forcing  function? 


(80) 


(81) 


(82) 


9.  McRuer,  Duane;  Ashkemas,  Irving;  and  Graham,  Dustan,  Aircraft  Dynamics 
and  Automatic  Control,  Princeton  University  Press. 
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The  equations  (3)  through  (7)  are  the  functional  relationships  that 
define  the  observation  data  in  spherical  with  respect  to  the  Cartesian 
coordinate  frame  or  in  the  Cartesian  frame  with  respect  to  the  elements  in  the 
spherical  frame.  These  equations  are  used  as  the  basis  for  the  noise  variance 
transformation  from  spherical  to  the  Cartesian  frame. 

The  differentials  of  equations  (3)  through  (5)  are: 


6*c  =  6R  cos6cos$  -  R66sin0cos$  -  RitycosQs i n$ 

6yc  =  6R  sin6cos$  ♦  R60cos0cos$  -  R6$sin0sin$  (86) 

f>zc  =  -6R  sin$  -  R6<)>cos$ 

The  noise  variance  can  now  be  defined  as: 


E[6*c2]  =  oR2cos20  cos2$  +  R2nq2sin02cos20  +  o^2R2cos20sin2$ 

E[6 y  2]  =  aD2sin20  cos2<p  +  R2a„2cos20cos2$  +  R2o  2sin20sin2$  (87) 

C  K  U  <p 

E[6z  2]  =  o„2sin20  +  R2cr  2cos2<{> 

C  K  (p 

where 


oR2  -  E[6R2  ] 

o02  =  E(602 ]  (88) 

o^2  =  E[6*2] 

The  linear  Kalman  filter  will  be  completely  defined  once  the  initial 

conditions  are  defined.  The  initial  conditions  needed  to  start  the  filter  are 

x  ,  P  ,  and  Q  . 
o’  o’  ^o 


The  initial  estimate  of  the  state  vector  is  determined  as  follows.  The 
position  element  of  the  state  vector  will  be  the  second  usable  measurement  of 
position  from  the  sensor.  Hence,  a  two  point  start  is  assumed.  The  velocity 
component  is  approximated  using  Euler's  definition  of  the  derivative  from  the 
first  and  second  measurements  of  position.  The  initial  values  of  the  elements 
of  the  covariance  matrix  are  calculated  as  follows;  with  covariance  matrix 
defined  as 


P  =  E[6x  6xT ) 


where 

6x  = 

P  = 

E 


6x  6x„  6x  6x„ 
c  c  c  c 

6x  6x„  6x  6x 
c  c  c  c 


(89) 

(90) 

(91) 
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6xc  is  defined  in  equation  (86)  using  the  definition  for  the  Euler  integration 


6xc(V  3  6xc(tk)  '  6xc(tk-l> 

- T - 


(92) 


E[6i  (t. )2]  =  2q6x2 

c  *  ~ET~ 


(93) 


where 


o6x2  =  E[6x  fix] 


(94) 


Using  similar  logic 

E[6x  (tk)  6x  (tk)]  =  °6x2 

A 


Thus 


P 


o 


2 


°6xc 

A 

2o6xc 


A 


(95) 


(96) 


This  completes  derivation  of  the  decoupled  linear  (Cartesian  coordinate) 
filter  algorithm.  The  equations  for  this  filter  algorithm  are  those  of  table 
1  where  the  following  initial  condition  and  assumptions  were  made: 


1.  Linear  dynamics  with  state  transition  matrix 


<t>  = 


1  A 
0  1 


2.  Forcing  function  distribution  matrix 


G  = 


0 

1 


3.  Forcing  function  is  missile  translational  accelerations  measured  by 
onboard  accelerometers 


4.  w(t)  is  accelerometer  sensor  noise  variance 

5.  Ground  tracking  radar  sensor  noise  variances  in  Cartesian 
coordinates  are  defined  in  equation  (92) 
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6.  Initial  value  of  x  is  derived  from  the  first  two  consecutive 
useable  measurements  of  the~observation  data  and  initial  value  of  P,  P  is 
defined  in  equation  (96) 

5.2  EXTENDED  KALMAN  FILTER  APPROACH 

The  second  approach,  which  is  the  recommended  approach  and  implemented  as 
a  computerized  algorithm,  is  the  nonlinear  extended  Kalman  filter  (linear 
dynamics  but  nonlinear  measurements)  implemented  in  Cartesian  coordinates. 

The  state  vector  x(t),  of  the  dynamic  system  (missile)  in  flight  is  defined  as 


(97) 


and  the  state  evolves  as  the  following: 

x(t)  =  Fx(t)  +  Bu(t)  +  w(t)  (98) 

where 

x(t)  is  the  n  x  1  vector  of  system  state  variables, 

u(t)  is  the  P  x  1  vector  of  deterministic  forcing  functions, 

w(t)  is  the  q  x  1  vector  of  the  random  forcing  functions, 

F  is  the  n  x  n  system  matrix, 

G  is  the  n  x  P  distribution  matrix  for  deterministic  forcing. 

The  solution  to  (98)j  which  is  used  to  propagate  the  state  vectoryis 

x(tk~)  =  <t>(tk,  +  J  4>(tk_1,T)Gu(T)dT 

tk-l 

lk 

+  J  <K(tk.  t)w(t)dt  (99) 

tk-l 


x(t)  = 


LZc. 
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where 


(100) 


3<t>^,T)  =  F4»(t,T),  4»(t,t)  =  I 


and 


<D(t,t0)  =  L_1[(SI  -  F)"1] 
and  the  system  matrix,  F,  is 


F  = 


0  0  0  1  0  0 

0  0  0  0  1  0 

0  0  0  0  0  1 

0  0  0  0  0  0 

0  0  0  0  0  0 

.0  0  0  0  0  0. 


(101) 


(102) 


The  solution  to  the  differential  equation  defined  in  (55)  which  propagates  the 
covariance  matrix  is  (see  ref  5) 

p(tk")  =  tk-l)  P(tk-l+)4>T(tk’  tk-l) 

^k  T 

+  J  *(tk,  t)Q(t)1>'(tk,  t)dt  (103) 

tk-l 

The  G  matrix  is  defined  as 


G  = 


*  0  0  0' 
0  0  0 
0  0  0 
10  0 
0  10 
0  0  1 


(104) 


which  is  the  distribution  matrix  for  forcing  function,  missile  acceleration, 
defined  as 


u(t)  = 


and 


(105) 


w(t) 


0 

0 

0 


(106) 
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which  is  the  random  forcing  of  the  system. 


The  variance  of  the  random  forcing  Q(t)  is  defined  as 
Q(t)6(t  -  x)  =  E  {w(t)wT(x)} 


Q  =  E 


0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

6ax2 

6ax6ay 

6a  6a 

X 

0 

0 

0 

6a  6a 

y  x 

6ay2 

6a  6a 

y 

0 

0 

0 

6az6ax 

6az6ay 

6az2 

The  measurement  equation  is  given  as 
z(tk)  =  h(*(tk))  +  v(tk) 
with  the  partials,  H(x(tk-),  defined  as 

_  3h(x) 


H(x(V)) 


ax 


x  =  x(tk-) 


and  restating  the  functional  relationships , 
h(x(tk)) ,  as 


R(t.)  =  (x  2(t.  )  ♦  y  2(t  )  ♦  z  2(t  ) 


1/2 


0(tk)  =  ATAN(yc(tk)/Xc(tk)) 


♦(tR)  =  -ATAN 


(107) 


(108) 


(109) 


(110) 


’zc‘tk>(xc2(tk>  *  W)'1/2]  <nl> 


The  partials  which  comprise  the  elements  of  the  measurement  matrix  are: 


Hd,!)  =  3R(x>y*z) 
9x 

h(i,2)  =  9R<x>y»z> 

ay 

H(  1,3)  = 

dz 

H(l,4)  = 

ax 


=  y-)(v<-)  *  h!<->  *  v<->) 

=  »k<->(v<->  *  v<->  *  2k2<->) 

= .  =  2k<->(*k2<->  *  K2<->  ♦  2k2‘->) 


x  =  xk(-) 


X  =  xk(-) 


1/2 


-1/2 


-1/2 


=  0 


x  =  xk(-) 


(112) 
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(112) 

H(3,5)  = 

sy 

H(3,6)  = 

3z 

The  update  equations  for  the  extended  Kalman  filter  are: 


K(tk)  =  P(tk-)HT(x(tk-))|^H(x(tk-))p(tk-)HT(x(tk-) 

)  +  R(tk)j_1  (113) 

x(tk+)  =  x(tk-)  +  K( tk)  z( tk)  ~h(x(tk~)^ 

(114) 

P(tk+)  =  I  -  K(tk)  H(x(tk-))  P(tk-) 

(115) 

The  filter  was  initialized  using  the  same  procedure  as  defined  for  the 
strictly  linear  Kalman  filter  described  earlier  in  this  section.  The  initial 
covariance  matrix,  however,  was  a  6  x  6  matrix  rather  than  a  2  x  2.  The 
initial  covariance  matrix  was 


fix  fix 
c  c 

6xc6yc 

6xc6zc 

6xc6ic 

6xc6yc 

6xc6zc 

fiy  6x„ 
c  c 

«yc6yc 

6yc6zc 

6yc6*C 

6*c6*c 

6yc6zc 

Po  =  E 

fizfix 
c  c 

6zc6yc 

6zc6zc 

fiz  fix 
c  c 

6zc6*c 

5zc6zc 

(116) 

fix  6x„ 
c  c 

6xcdyc 

6xc6zc 

6icK 

6xc6yc 

6xc6zc 

syc6*c 

6yc6yc 

6y  6z 
c  c 

«yc6ic 

6yc6zc 

_6ic6xc 

6zc6yc 

fiz  fiz 
c  c 

fiz  fix 
c  c 

fiz-fiy^ 
c  c 

6zc6zc 

In  the  abgve  equation  fix, 

fiy,  and 

fiz  are 

defined 

as  per  equation  (86). 

fix, 

fiy,  and  fiz  are  defined  as  in  the  Euler  approximations  to  the  derivative.  The 
evaluation  of  each  term  is  performed  by  first  multiplying  the  appropriate  term 
and  then  taking  the  expectation  of  the  products.  It  is  easily  seen  that  the 
algebra  gets  very  involved,  thus  an  approximation  for  obtaining  P  was 
investigated  for  the  analysis  in  this  study.  1 


continued 


=  0 


=  0 
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6.  ANALYSIS  OF  RECOMMENDED  APPROACH  AND  RESULTS 


The  recommended  approach  for  deriving  estimates  of  the  state  vector  which 
defines  the  trajectory  of  the  missile  and  or  target  in  free-fiight  is  the 
extended  Kalman  filter  as  described  in  the  previous  section.  This  approach 
was  studied  using  the  methodology  of  figure  4,  i.e.,  a  six  degree  of  freedom 
simulation  was  used  to  simulate  the  missile  in  free  flight.  Missile 
translational  acceleration  data  as  defined  by  equations  (85)  comprises  the 
forcing  function.  A  ground-based  simulated  radar  generated  tracking  or 
observation  data  as  the  missile  pursues  the  target.  A  series  of  analyses  were 
conducted  using  a  baseline  missile/target  intercept  geometry.  Filter 
performance  was  investigated  as  a  function  of:  (1)  the  initial  condition  xQ, 
and  P  ,  (2)  weighting  the  covariance  matrix,  and  sensor  noise  statistics. 

6.1  BASELINE  MISSILE  AND  TARGET  INTERCEPT 

The  initial  values  for  the  missile  and  target  baseline  flight  simulator 
are  outlined  in  table  3. 


Missile: 

xm 

0.0  feet 

0.0  feet 

2m 

60,000.0  feet 

Speed  at  Launch 

Mach  0.9 

Target: 

XT 

84,000.0  feet 

*T 

30,573.0  feet 

ZT 

60,000.0  feet 

Seed  (Constant  Velocity) 

Mach  0.8 

Table  3.  Flight  simulation  baseline  values. 

A  diagram  of  the  XY  plane  of  the  missile  target  intercept  geometry  is 
illustrated  in  figure  7.  Since  the  missile  and  target  were  at  a  co-altitude 
for  the  launch,  the  XY  plane  is  the  plane  of  interest.  This  baseline 
trajectory  was  used  for  all  the  filter  performance  analysis  that  was 
conducted. 
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Y  COORDINATE -FEET 


(INTERCEPT)  (LAUNCH) 

X  COORDINATE  — FEET 

Figur°  7.  Missile/target  intercept  geometry.  (XY  plane  of  trajectory) 


6.2  FILTER  PERFORMANCE  ANALYSIS 

Figures  8  through  16  present  performance  analysis  data  for  the  extended 
Kalman  filter.  Figures  17  through  19  are  comparative  data  of  the  linear 
filter.  Two  sets  of  statistical  performance  parameters  (variance  on  range, 
azimuth,  and  elevation)  were  used  for  the  analysis.  Ref  (11)  defines  the 
noise  standard  deviations  for  two  radars  at  the  White  Sands  Missile  Range. 
The  standard  deviations  (sigmas,  "a")  are  as  follows: 


°R 

°A 

°E 

FPS-16's 

5  ft 

.1  mil 

.1  mil 

MPS-36's 

5  ft 

.2  mil 

.2  mil 

The  radar  statistical  error  data  used  in  this  analysis  was  defined  as  a  low 
error  case  and  a  large  error  case.  Even  the  low  error  case  was  of  a  higher 
noise  level  than  that  of  ref  11.  This  was  because  the  objective  of  the 


11.  Mathematical  Service  Branch,  U.S.  Army  White  Sands  Missile  Range, 
Technical  Report  No.  58,  Optimal  Radar  Instrumentation  Planning,  by 
William  S.  Agee,  White  Sands  Missile  Range,  New  Mexico. 
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analysis  was  to  thoroughly  evaluate  the  filter's  capabilities  operating  with 
poor  observation  data--the  theory  being  that  if  the  filter  can  obtain 
reasonable  estimates  with  very  noisy  observations,  it  will  yield  better 
estimates  with  less  noisy  observation  data.  The  statistics  defining  the  two 
individual  cases  are  as  follows: 


Low  Noise 
(case  1) 


10  ft 


,316  mil 
Rad 


316  mil 
Rad 


High  Noise  31.62  ft  3.16  mil  3.16  mil 
(case  2)  Rad  Rad 


6.2.1  Extended  Kalman  Filter  Analysis 

Figures  8  through  11  present  data  for  Case  1  (x  component).  Figure  8 
presents  two  plots:  (1)  error  of  actual  raw  data,  and  (2)  error  in  the 
estimated  value  of  the  x  component  of  the  missile  flight  trajectory.  The 
curves  clearly  illustrate  that  a  large  reduction  in  the  measurement  error  is 
obtained  by  filtering.  Figure  9  presents  four  data  plots.  These  are  plots  of 
the  RMS  errors  z  -  x,  z  -  x,  and  x  -  x.  The  method  of  computing  the  standard 
deviation  of  sampled  data  was  that  of  ref  12,  i.e., 


(xp2 


i  =  1 


(117) 


In  figure  9,  two  curves  for  the  RMS  error  of  x  -  x  are  plotted.  One  curve 
starts  the  RMS  formula  of  equation  (11)  at  time  equal  to  1  second;  neglecting 
the  data  from  time  zero  up  to  1  second.  The  second  curve  starts  the  RMS 
formula  at  time  equal  to  6  seconds,  i.e. ,  i  =  1  at  t  =  6.  The  first  6  seconds 
of  data  are  not  weighted  in  the  RMS  calculation.  It  was  noted  that  the 
convergence  time  of  the  filter  (with  the  diagonal  elements  of  the  Q  matrix 
being  1  x  104)  was  roughly  6  seconds.  Figure  9  clearly  indicates  the  level  of 
performance  of  the  extended  Kalman  filter.  The  estimated  values  of  the  x 
component  of  the  trajectory  are  four  to  six  times  better  than  the  raw  data 
measurements  of  the  x  component  of  the  missile  trajectory.  This  is  based  on 
the  ratio  of  RMS  errors. 


12.  Bendat,  Julius,  and  Piersol,  Allan  G. ,  Random  Data  Analysis  and 

Measurement  Procedures,  Wi ley-Interscience,  a  Division  of  John  Wiley  and 
Sons,  Inc.,  New  York,  N.Y.,  1971. 
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Figure  8.  Actual  measurement  error  and  error  of  estimated  state 
(X  component)  vs  time. 
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Figure  9.  RMS  errors  for  extended  Kalman  filter  (X  component)  vs  time. 
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Figures  10  and  11  illustrate  RMS  error  curves  for  the  low  noise  case 
where  the  diagonal  elements  of  Q  have  been  varied.  In  figure  10  the  diagonal 
elements  are  1  x  10s.  In  figure  11  the  diagonal  elements  of  the  Q  matrix  are 
1  x  104.  It  is  quite  evident  that  the  Q  matrix  plays  an  important  role  in 
estimating  the  elements  of  the  state  vector.  It  was  noted  that  the  conver¬ 
gence  rate  of  the  estimator  was  higher  for  higher  values  of  the  Q  matrix. 

This  did  not  necessarily  mean  better  estimates.  It  was  found  that  there  was 
an  optimum  value  for  the  diagonal  elements  of  the  Q  matrix  which  yielded  "best 
estimates. " 

Figures  12  and  13  are  the  data  for  Case  1,  the  low  noise  case,  of  the  y 
component  of  the  position  element  of  the  state  vector.  An  interesting  aspect 
is  noted  from  this  data--the  geometry  has  a  large  impact  on  the  noise  level  of 
the  y  component  of  the  sensor  data.  Again,  the  filter  does  a  good  job  of 
obtaining  estimates  of  the  y  element  of  the  state  vector.  The  ratio  is 
approximately  4  to  1. 

Figures  14  through  16  present  data  on  Case  2  (the  high  level  noise  case) 
for  the  x  component  of  the  state  vector.  For  comparison,  compare  figure  14  to 
figure  8  and  figure  15  to  figure  9.  It  is  quite  obvious  that  the  filter  does 
an  excellent  job  in  obtaining  estimates  of  the  position  elements  of  the  state 
vector.  It  can  be  seen  by  studying  figure  14  that  actual  errors  can  be  as 
large  as  450  feet  while  the  estimates  are  under  80  feet.  The  RMS  data  of 
figure  15  is  quite  interesting.  The  RMS  values  were  calculated  using  equation 
6.1  for  data  after  t  =  6  seconds.  That  is,  the  data  before  time  reached  6 
seconds  was  not  included  in  the  RMS  calculations.  The  estimates,  for  the  data 
of  figure  15  were  three  to  four  times  better  than  the  actual  raw  measurements. 
The  values  of  the  elements  along  the  diagonal  of  the  Q  matrix  were  varied  from 
1  x  104  for  the  data  of  figure  15  to  1  x  10s  for  the  data  of  figure  16.  The 
estimates  were  closer  to  the  actual  values  of  the  state  vector  (x  component) 
by  a  factor  of  approximately  1.25.  Thus  the  same  phenomenon  is  noted  for  the 
high  noise  case  as  for  the  low  noise  c.i'3e'--fhe  estimates  can  be  made  closer  to 
the  actual  values  by  weighting  the  covariance  matrix. 

6.2.2  Linear  Kalman  Filter  Analysis  Results 

Figures  17,  18  and  19  present  data  on  the  performance  of  the  decoupled 
linear  Kalman  filter  where  the  noise  variance  of  the  observation  data  was 
transformed  via  equations  (87)  from  spherical  to  Cartesian  reference  frames. 
Figure  17  presents  the  curves  for  the  error  in  the  actual  data  and  the  error 
in  the  estimated  value  of  the  state.  The  decoupled  linear  Kalman  filter  did 
not  perform  very  well  as  illustrated  in  figure  17.  The  error  in  the  estimated 
state  was  as  large  or  larger  than  the  error  in  the  observation  data.  The  RMS 
values  of  the  errors  (x  -  x,  and  z  -  x)  are  presented  in  the  curves  of  figure 
18.  The  figure  illustrates  that  the  estimates  were  poorer  than  the  raw  data. 
Again,  the  diagonal  elements  of  the  Q  matrix  were  varied.  It  can  be  seen  from 
the  data  of  figure  18  that  the  filter  performance  can  be  improved  by  varying 
the  elements  of  the  Q  matrix. 

Figure  19  presents  the  RMS  error  data  for  the  y  component  of  the  position 
element  of  the  state  vector.  Again,  it  is  noted,  as  was  the  case  of  the 
extended  Kalman  filter,  that  the  error  in  the  observation  data  is  a  function 
of  the  geometry.  The  error  increases  as  the  y  component  of  the  state  vector 
increases. 
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Figure  10.  RMS  errors  for  extended  Kalman  filter  (X  component)  vs  time. 
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Figure  11.  RMS  error  for  extended  Kalman  filter  (X  component)  vs  time. 
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gure  12.  Actual  measurement  error  and  error  of  estimated  state 
component)  vs  time. 
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Figure  13.  RMS  error  for  extended  Kalman  filter  (Y  component)  vs  time 
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7.  CONCLUSIONS  AND  RECOMMENDATIONS 

The  validation  of  missile  system  performance  specifications  from  test 
data  (captive  and  free-flight)  requires  a  high  level  of  confidence  in  the 
observation  data  recorded  during  the  test.  This  study  was  directed  at  raising 
the  confidence  level  of  that  test  data. 

The  methodology  of  approaching  this  problem  is  outlined  in  figure  4  which 
illustrates  two  areas  of  effort  as  delineated  by  the  two  differently  shaded 
areas:  (1)  completed  effort  and  (2)  recommended  follow-on.  The  initial 
effort  conducted  was  the  development  of  a  sequential  tracking  algorithm  for 
two  different  approaches,  the  validation  of  the  tracking  algorithm  using  a 
6-degree  of  freedom  missile  simulation  to  supply  the  simulated  measurements  of 
the  missile  in  flight,  and  the  recommendation  of  the  preferred  approach  based 
on  comparative  analysis. 

The  two  different  algorithms  that  were  implemented  and  validated  were  (1) 
the  decoupled  linear  Kalman  filter  and  (2)  the  extended  Kalman  filter 
(nonlinear  measurements). 

7.1  CONCLUSIONS 

The  performance  of  the  extended  Kalman  filter  was  superior  to  that  of  the 
linear  filter  approach,  hence  the  recommended  approach  is  the  extended  Kalman 
filter  working  in  a  Cartesian  reference  frame  with  nonlinear  spherical 
coordinate  frame  observation  data.  The  extended  Kalman  filter  worked  very 
well  with  high-  and  low-level  noise  observation  data.  The  estimates  were  reduced 
three  to  five  times  (on  an  RMS  ratio  basis)  than  the  raw  measurement  data. 

The  recommended  filter's  performance  was  a  function  of  a  weighting  of  the 
covariance  matrix.  This  weighting  affected  the  convergence  rate  and  the  level 
of  the  estimates  at  filter  turn-on  and  during  the  period  just  following  filter 
turn-on.  The  R  matrix  for  the  extended  Kalman  filter  was  constant  as  opposed 
to  the  R  matrix  which  varied  as  a  function  of  time  for  the  linear  filter 
approach. 

The  use  of  an  extended  Kalman  filter  as  defined  in  this  report  greatly 
enhances  the  level  of  confidence  in  defining  the  missile's  free-flight 
trajectory-raw  data  errors  (RMS)  on  the  order  of  20  feet  are  reduced  to  RMS 
errors  of  5  feet  or  less! 

7.2  RECOMMENDATIONS 

It  is  recommended  that  a  system  engineering  effort  be  conducted  as 
outlined  by  the  shaded  areas  identified  as  "follow-on"  in  figure  4.  This 
effort  would  be  in  the  following  areas: 

(1)  Adaptive  Kalman  filtering 
(a)  Maneuver  gates 
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(b)  Weighted  gain  matrix 


(2)  Initialization 

(a)  Timing 

(b)  Initial  values  of  covariance,  state  vector,  and  measurement 
sensor  noise 

(3)  Terminal  intercept  performance 

(a)  Variance  of  1 ine-of-sight  rates  measured  by  guidance  sensors 

These  three  areas  would  be  the  primary  areas  of  research  and  analysis. 
Secondary  areas  of  investigation  would  be: 

(4)  Nonlinear  dynamics 

(5)  Smoothing  of  optimal  estimates,  and 

(6)  Multiple  sensor  observations. 

Some  interesting  observations  were  made  during  the  first  study  phase  at 
the  Naval  Ocean  Systems  Center,  relative  to  adaptive  Kalman  filters.  It  was 
noted  that  by  varying  the  covariance  matrix  during  the  initialization  phase  of 
the  trajectory  that  the  rate  of  conveyance  of  the  estimated  values  of  the 
state  vector  could  be  controlled.  This  was  an  open  loop  form  of  control.  One 
of  the  adverse  effects  was  that  the  filter  could  become  unstable.  A 
methodology  was  investigated,  but  not  implemented,  that  would  incorporate  a 
form  of  closed  loop  control  on  the  initial  variation  of  the  covariance  matrix. 
This  in  effect  would  be  an  adaptive  filter.  The  gain  matrix  would  then  be 
weighted  as  a  function  of  the  variations  in  the  covariance  matrix.  An 
algorithm  would  be  developed,  if  appropriate,  to  effect  this  closed  loop 
control  of  the  Kalman  filter  convergence.  Other  schemes  to  be  investigated  in 
formulating  the  adaptive  filter  would  be  to  monitor  the  residuals  to  determine 
if  a  large  maneuver  had  occurred.  The  observation  data  would  then  be  weighted 
relative  to  the  estimated  maneuver  before  being  processed  by  the  Kalman 
filter. 

Initialization  of  the  filter  is  another  area  of  recommended  research. 
Methodology  for  determining  when  to  start  the  filter  and  how  to  estimate  the 
initial  values  of  the  covariance  matrix  and  initial  values  of  the  state  vector 
should  be  investigated. 

The  final  area  of  recommended  research  is  to  analyze  the  terminal 
intercept  performance.  Techniques  for  determining  accurate  values  of  the 
1 ine-of-sight  rates  as  observed  by  the  guidance  sensor  during  the  final  3000 
feet  of  flight  trajectory  should  be  analyzed.  The  problem  associated  with 
determining  accurate  1 ine-of-sight  rates  (LOSR)  from  observation  data  of  the 
reconstructed  missile  and  target  flight  trajectories  is  that  the  LOSR  is  a 
function  of  one  over  the  square  of  relative  range  between  the  missile  and 
target.  As  this  range  goes  to  zero  the  LOSR  goes  to  infinity.  When  there  is 
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a  large  uncertainty  in  the  relative  range  the  variance  of  the  LOSR  becomes 
large.  This  is  the  reason  why  the  estimates  of  the  trajectories  need  to  be 
optimized  with  the  least  error  variances.  The  reason  for  determining  the  LOSR 
is  to  compare  it  to  measured  1 ine-of- sight  rates  as  observed  by  the  guidance 
sensor  for  performance  evaluation.  If  the  methodology  for  determining  the 
actual  line-of-sight  rates  has  a  higher  error  variance  than  the  variance  of 
the  guidance  sensor,  it  makes  little  sense  to  do  a  performance  evaluation. 

The  standard  must  have  a  higher  level  of  confidence  associated  with  it  as 
compared  to  the  item  being  compared  against  the  standard.  A  possible  solution 
to  this  problem  would  be  to  utilize  the  inertial  reference  system  on  board  the 
missile  itself.  Alignment  errors  would  be  estimated  during  the  initial  and 
midcourse  phases  of  the  flight  trajectory;  therefore,  during  the  terminal 
intercept  phase  a  higher  degree  of  confidence  in  the  missile's  trajectory 
could  be  realized. 

Secondary  areas  of  recommended  research  that  should  be  investigated  deal 
with  optimal  smoothing,  nonlinear  dynamics  and  multiple  sensors  to  collect  the 
observation  data.  A  brief  outline  of  these  areas  of  research  follows. 

The  initial  study  performed  at  the  Naval  Ocean  Systems  Center  utilized 
linear  dynamics  with  nonlinear  measurements  in  the  extended  Kalman  filter 
model.  The  assumption  was  made  that  the  acceleration  data  was  available  at  a 
high  data  rate  and  therefore  constant  over  the  sampling  interval.  The 
transformations  of  the  acceleration  from  missile  body  to  inertial  reference 
frames  were  done  outside  the  filter.  If  a  reference  frame  were  utilized  where 
the  system  dynamics  was  a  nonlinear  combination  of  the  states  the  extended 
Kalman  filter  would  have  to  be  modified  to  incorporate  the  nonlinear  system 
dynamics. 

Optimal  smoothing  of  data  is  another  means  of  processing  raw  data.  This 
smoothing  process  can  also  be  applied  to  data  that  has  been  processed  by  a 
sequential  estimation  filter.  This  area  of  research  would  investigate 
alternate  smoothing  techniques  for  processing  the  observation  data. 

Multiple  sensor  observation  is  an  extension  of  the  single  sensor 
problem.  The  data  from  the  ground-based  sensors  positioned  at  various 
locations  throughout  the  test  range  would  each  generate  an  estimate  of  the 
missile's  and  target's  trajectory.  These  trajectories  could  then  be  processed 
through  a  smoother  to  arrive  at  a  finalized  version  of  the  trajectories. 
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APPENDIX  A 


DEFINITIONS  OF  COMPUTER  PROGRAMS 
A. 1  OVERVIEW  OF  6-DOF  TRAJECTORY  SIMULATION 
A. 1.1  General  Program  Flow 

The  program  is  broken  into  seven  stages,  indicated  by  variable  IENTRY,  as 
fol lows: 

IENTRY  STAGE  DESCRIPTION 

1  Pre-data  initialization 

2  Post-data  initialization 

3  Trajectory  computation 

4  Not  currently  used 

5  Not  currently  used 

6  Periodic  print  out 

7  Post-flight  computations,  print  and  plot 

The  program  routines  involved  in  each  stage  are  shown  in  the  following 
diagrams. 


IENTRY  =  1  &  2 
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I  ENTRY  =  7 


A. 1.2.  Subroutine  Descriptions 


MAIN  --  Allocates  storage,  defines  error  tolerances,  and  sets  up  initial 
indices  for  the  integration  routine. 

INPUT  --  Reads  and  stores  input  data 

INIAL  --  Computes  initial  parameters  from  inputs 

BLK1  --  BLOCK  DATA--Constants  and  reference  values 

BLK2  --  BLOCK  DATA--Thrust  tables 

BLK3  --  BLOCK  DATA — Aerodynamic  coefficients 

DERIV  --  Calls  routines  to  compute  differential  equations 

CNTRL  --  Controls  program  flow  as  to  breakup  times,  print  intervals  and 
program  stops. 

TARGET  --  Computes  target  trajectory 

RELT  --  Relative  missi le-to-target  kinematics 

AUTS  --  Model  of  control  system  (auto  pilot) 

SACS  --  Aerodynamic  forces  and  moments 

GUIDE  --  Model  of  missile  guidance  scheme 

FCC  --  Fire  control  computer  logic 

VPCS  --  Vehicle  physical  characteristics 

TFFS  --  Computes  thrust  components 

RKS4  --  Numerical  integration  routine--Fourth  order  Runge-Kutta  with 
Simpsons  Rule  check--Variable  or  fixed  interval. 

EQOM  --  Equations  of  motion 

PRINT  --  Basic  trajectory  print  out 

ATMOS  —  Computes  atmospheric  pressure  and  velocity  of  sound  from  IACO  1962 
model  atmosphere. 

INVR,  M3X31,  TMULT  —  Matrix/vector  manipulation 
L00K1,  L00K2,  L00K3  —  Lookup  routines  for  tabular  data 
GAUSS,  RANDU  —  Noise  generator 
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A.  2  KALMAN  FILTER  ROUTINE  DESCRIPTIONS 


A. 2.1  General  Description  and  Use  of  Kalman  Filter  in  6-DOF 

A  tracking  type  Kalman  filter  was  inserted  into  the  trajectory  simulation 
to  test  the  filter's  accuracy.  Uncluttered  trajectory  information  was  taken 
from  the  simulation,  Gaussian  noise  was  added  and  the  results  were  fed  into 
the  filter.  The  filter  had  no  effect  on  the  trajectory. 

The  software  for  the  Kalman  filter  consisted  of  the  following  routines: 

KALDRV  —  driver  routine  to  set  up  observation  and  forcing  function. 

KALFT  --  Kalman  filter  computations 

ACXFM  —  adds  noise  to  the  missile  body  x,  y,  z  accelerations  then  transforms 
them  to  inertial  coordinates. 

MMULT  --  performs  matrix  multiplications 

DINVER  --  performs  matrix  inversion 

MATIO  --  performs  matrix  input/output 

The  filter  is  connected  to  the  simulation  through  the  CNTRL  routine  which 
calls  KALDRV.  Logic  was  added  to  the  CNTRL  routine  to  call  KALDRV  in  stage  2 
and  then  to  call  KALDRV  in  stage  3  at  time  zero  and  thereafter  every  DTKAL 
seconds.  DTKAL  is  an  input  variable  which  indicates  the  Kalman  filter  time 
interval.  CNTRL  also  tells  KALDRV  to  print  filter  information  at  the  same 
interval  as  the  simulation. 

A .2.2  Kalman  Filter  Routine  Documentation  (Extended  Kalman  Filter) 

A.  KALDRV 

1.  USAGE:  Call  KALDRV  (TIME,  IKALP,  DTKAL) 

2.  PURPOSE:  To  initiate  and  drive  the  Kalman  filter. 

3.  SUBROUTINES  REQUIRED: 

MATIO  --  matrix  input/output  routine 

MMULT  --  matrix  multipl ication  routine 

GAUSS  --  Gaussian  distribution  random  number  generator 

ACXFM  --  routine  to  set  up  the  forcing  function 

KALFT  --  the  Kalman  filter  calculations 


ARGUMENTS: 


Input:  TIME  —  trajectory  simulation  time,  in  seconds 

IKALP  --  print  flag,  IKALP  =  0  -»  output  filter  data, 
IKALP  =  1  -»  call  filter  but  perform  no 
output 

Output:  DTKAL  —  Kalman  filter  time  interval,  in  seconds 

COMMON  VARIABLES:  all  variables  in  COMMON  are  inputs. 

COMMON  AREA  ENT 

IENTRY  --  indicates  the  stage  of  the  trajectory  simulation 
COMMON  AREA  DER 

DY  --  array  of  simulation  derivatives 

Y  --  array  containing  the  results  of  integrating  DY 

Note  that  only  three  variables  from  each  Y  and  DY  are  used, 
namely  Y(4),  Y(5),  Y(6)  and  DY(4),  DY(5),  DY(6).  These  are 
equivalenced  to  XG,  YG,  ZG  and  XGDOT,  YGDOT,  ZGDOT, 
respectively,  and  are  the  missile  position  and  velocity  vectors 
in  the  inertial  (ground)  coordinate  system. 


6.  FLOWCHART: 


51 


7.  OUTPUTS:  all  inputs  are  echoed  on  output 


R,  THETA, 

z 

FF 

x 

OBXYZ  -- 

zMx 

RMS 

OBXMXG  — 

OBXMX  — 

RMS1  — 

RMS2  — 

PK 


PHI  --  position  of  missile  in  spherical  coordinates, 
without  noise 

observation  vector,  three  element  vector  containing 
the  missile  position,  in  spherical  coordinates. 

forcing  function  vector,  three  element  vector 
containing  the  missile  x,  y,  z  accelerations,  in  the 
inertial  coordinate  system,  with  noise 

estimated  state  vector,  six  element  vector  containing 
the  estimated  state  from  the  Kalman  filter,  x(l), 
x(2),  x(3)  are  the  estimated  coordinates  of  position 
and  x(4),  x(5),  x(6)  are  the  estimated  velocities, 
both  in  rectangular  coordinates 

three  element  vector  containing  the  observation  in 
rectangular  coordinates 

six  element  vector  containing  the  actual  value  of 
each  state,  from  the  simulation,  minus  the  estimated 
value  from  the  Kalman  filter 

six  element  vector  containing  the  accumulated  RMS 
error  (actual  minus  estimated) 

three  element  vector  containing  the  actual  value  of 
position,  from  the  simulation,  minus  the  observed 
value  (actual  plus  noise),  in  spherical  coordinates 

three  element  vector  containing  the  observed  values 
of  position  minus  the  estimated  values  (i.e., 

OBXYZ(i)  -  X(i),  i  =  1,  2,  3) 

three  element  vector  containing  the  accumulated  RMS 
error  of  position  (actual  minus  observed) 

three  element  vector  containing  the  accumulated  RMS 
error  of  position  (observed  minus  estimated) 

the  current  value  of  the  error  covariance  matrix 
associated  with  the  current  estimate  of  the  state 
vector 

the  current  value  of  the  Kalman  gain  matrix 
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B.  ACXFM 
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USAGE:  CALL  ACXFM  (IX,  IKALP,  VAX,  VAY,  VAZ,  AX,  AY,  AZ) 

PURPOSE:  To  calculate  the  missile  accelerations,  with  noise, 
in  the  inertial  coordinate  system. 

SUBROUTINES  REQUIRED: 

GAUSS  --  Gaussian  distribution  random  number  generator 
ARGUMENTS: 

Input:  IX  —  seed  for  GAUSS 

IKALP  —  print  flag,  IKALP  =  1  -*  print, 

IKALP  =  0  ■*  no  print 

VAX,  VAY,  VAZ  --  standard  deviations  for  X,  Y  and  Z 
acceleration  noises 

Output:  AX,  AY,  AZ  —  X,  Y  and  Z  components  of  acceleration 

in  the  inertial  coordinate  system  with 
noise. 

COMMON  VARIABLES:  all  variables  in  COMMON  are  input 
COMMON  AREA  PPP 

THETAP,  PSIP,  PHIP  --  missile  Euler  angles 

GX,  GY,  GZ  --  X,  Y  and  Z  components  of  gravity  in  the  body 
system 

COMMON  AREA  OER 

DY  --  array  of  simulation  derivatives 

Y  --  array  containing  the  results  of  integrating  DY.  The 

elements  of  Y  and  DY  that  are  needed  are  equivalenced 
to  meaningful  variables  names  as  follows. 

Y(l)  =  U,  Y(2)  =  V,  Y(3)  =  W  --  these  are  the  missile  X,  Y  and 
Z  velocities  in  the  body  system. 

Y(7)  s  PP,  Y(8)  s  QQ,  Y(9)  =  RR  —  these  are  the  roll,  pitch 
and  yaw  angular  rates  in  the  body  system. 

DY(1)  =  UDOT,  DY ( 2 )  =  VDOT,  DY(3)  s  WDOT  —  these  are  the 

missile  X,  Y  and  Z  accelerations  in  the  body  system. 


5  FLOWCHART 

GIVEN  4>  -  THE  TRANSITION  MATRIX  OF  THE  SYSTEM 

Q  -  COVARIANCE  MATRIX  OF  RANDOM  FORCING  FUNCTION  NOISE 
R  -  COVARIANCE  MATRIX  OF  RANDOM  OBSERVATION  NOISE 
tl K-t  -  FORCING  FUNCTION  VECTOR  OF  PREVIOUS  ITERATION 
XK-1(  • )  -  PREVIOUS  ESTIMATE  OF  STATE  VECTOR 
P«- 1  (  • )-  ERROR  COVARIANCE  MATRIX  OF  PREVIOUS  ESTIMATE 
Zk  -  CURRENT  OBSERVATION 


KALFT 


1.  USAGE:  CALL  KALFT  (IKALP,  PHI,  FF,  Q,  R,  Z,  P,  G,  X) 

2.  PURPOSE:  To  perform  one  iteration  of  the  tracking  type  Kalman 
filter  with  a  forcing  function. 

3.  SUBROUTINES  REQUIRED: 

MMULT  —  matrix  multiplication  routine 

OINVER  --  double  precision  matrix  inversion  routine 

4.  ARGUMENTS: 

Input:  IKALP  —  print  flag,  IKALP  =  1  ■*  print,  IKALP  =  0  -* 

no  print 

PHI  --  transition  matrix  of  the  system  (6x6) 

FF  --  forcing  function  vector  (3) 

Q  --  covariance  matrix  of  forcing  function 
statistics  (6x6) 

R  --  variance  matrix  of  observation  noise  (3x3) 

Z  --  observation  vector  (3) 

Output:  P  --  error  covariance  matrix  associated  with  the 

previous  estimate  of  the  state  vector  (6x6) 

G  --  Kalman  gain  matrix  (6x3) 

X  --  estimated  state  vector  (6) 


55 


6.  FLOWCHART 


APPENDIX  B 


COORDINATE  TRANSFORMATIONS 
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APPENDIX  B 


COORDINATE  TRANSFORMATIONS 

All  coordinate  transformations  used  in  this  simulation  are  composed  of 
standard  angles  and  rotations.  The  order  of  the  rotations  is  always  yaw, 
pitch  and  roll. 

Inertial  Axes  to  Missile  Body  Axes:  The  relationship  between  the  missile 
body  and  inertial  axes  systems  is  described  by  the  yaw,  pitch  and  roll  angles, 
as  shown  in  Figure  B.l.  The  transformation  is  given  by  the  following  matrix 
relation  (see  ref.  9): 


- —  — 
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’b 
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cosAsinY 

-sinA 
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sinrsinAcosH' 

-cosrsin'F 

sinrsinAsin'F 
+cosr  cosH* 

sinrcosA 

Jl 

^B 

cosPsinAcos*F 

+sinrsin4' 

cosrsinAsin'F 
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h 

_  _ 

^ , 

—  - 

^xB 


Figure  B.l.  Inertial  axes  to  missile  body  axes. 
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